// DSInvKine.cpp : rewrite inverse kinematic solver for DENSO in Cpp

// v1.0 (02/09/12): created

// v2.0 (18/09/12): split to cpp, hpp and write code in a class

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>

#include "MatWork.hpp"

#include "DENSOrobot.hpp"


using namespace cv;
using namespace std;

enum InputMode {READ_PENDANT, INV_KINE };


int run_RS232 = 1;  //0: NO; 1: YES
int mode = CTRL_MODE_XYZ;
InputMode ipm = READ_PENDANT;


int callJointPlanner(DENSOrobot DSrb)
{
	int ret = 1;

	Mat SOL, OrgPoseJnt;

	// ********  Test TrajPlanner
	printf("*************************\n");
	printf("Trajectory Planner Test\n");

	// define original joint pose from XYZ pose
	// this can be done by invkine or by reading the teaching pendant	
	if (ipm == INV_KINE)
	{
		double OrgPose_ar[6] = {120, -150, -180, 0, -CV_PI/2, 0};
		Mat OrgPose = Mat(1, 6, CV_64F, OrgPose_ar);
		DSrb.IKineSolve(OrgPose, SOL);  
		MATmtxdisp(SOL);
		Mat OrgPoseJnt;
		SOL.row(0).copyTo(OrgPoseJnt);
	} else
	{
		// READ_PENDANT mode
		double OrgPose_ar[6] = {CV_PI/4, 3*CV_PI/4, 0, 0, 0, 0};   // manually enter values here
		Mat OrgPose = Mat(1, 6, CV_64F, OrgPose_ar);		
		Mat OrgPoseJnt;
		OrgPose.copyTo(OrgPoseJnt);
	}

	// define destination pose in XYZ pose
	double DstPose_ar[6] = {150, -150, -180, 0, -CV_PI/2, 0};
	Mat DstPose = Mat(1, 6, CV_64F, DstPose_ar);
	Mat traj;

	//int ret = DSrb.TrajPlanner(OrgPoseJnt, DstPose, traj);   // BUG!! cannot return the right answer -> DONE already
	ret = DSrb.TrajPlanner(OrgPoseJnt, DstPose, traj, CTRL_MODE_XYZ);
	
	return ret;
}


int callXYZPlanner(DENSOrobot DSrb)
{
	int ret = 1;

	Mat OrgPoseJnt;

	// ********  Test TrajPlanner
	printf("*************************\n");
	printf("Trajectory Planner Test\n");

	// define original joint pose from XYZ pose
	// this can be done by invkine or by reading the teaching pendant	
	if (ipm == INV_KINE)
	{
		printf("mode not supported!");
		getchar();
		ret = 0;
		return ret;
		
	} else
	{
		// READ_PENDANT mode
		double OrgPose_ar[6] = {175.0, 0.0, 536.0, -CV_PI, CV_PI/2, CV_PI};   // manually enter values here
		Mat OrgPose = Mat(1, 6, CV_64F, OrgPose_ar);		
		Mat OrgPoseJnt;
		OrgPose.copyTo(OrgPoseJnt);
	}

	// define destination pose in XYZ pose
	double DstPose_ar[6] = {150.0, 50.0, 480.0, 0, CV_PI/2, 0};
	Mat DstPose = Mat(1, 6, CV_64F, DstPose_ar);
	Mat traj;

	DSrb.inputPose(OrgPoseJnt, CTRL_MODE_XYZ);  // check this func
	ret = DSrb.TrajPlanner(OrgPoseJnt, DstPose, traj, CTRL_MODE_XYZ);
	
	return ret;
}


int main()
{
    int ret;
		
	DENSOrobot DSrb = DENSOrobot(run_RS232);
			
	if (mode == CTRL_MODE_JOINT) 
	{
		// ******** Init joint0 
		double ang_ar[6] = {CV_PI/4, 0, 0, CV_PI/4, CV_PI/4, CV_PI/4};
		Mat ang = Mat(1, 6, CV_64FC1, ang_ar);
		
		// ********  Test Forward kinematic 
		Mat A;
		DSrb.FKineSolve(ang, A);
	
	} else
	{
	
		// ******** Init XYZ0
		double ang_ar[6] = {400, 0, 300, CV_PI/4, CV_PI/4, CV_PI/4};    // mm & rad
		Mat ang = Mat(1, 6, CV_64FC1, ang_ar);
		
		// ********  Test inverse kinematic 
		// Destination pose
		double pose_ar[6] = {150, -150, -180, 0, -CV_PI/2, 0};
		Mat fnlpose = Mat(1, 6, CV_64F, pose_ar);
		Mat SOL;
		DSrb.IKineSolve(fnlpose, SOL);
		MATmtxdisp(SOL);
		// getchar();

	}
	
	if (mode = CTRL_MODE_JOINT) 
	{
		ret = callJointPlanner(DSrb);

	} else
	{
		ret = callXYZPlanner(DSrb);
	}

	
	if (ret)
	{
			//MATmtxdisp(traj);
	}
	else
			printf("there is no good solution!!");

	getchar();

    return ret;
}
